/**
 * @brief 接受定位模块和规划模块的信息，得出底盘整体的运动速度控制指令
 *        然后进行运动学分解，得出每个轮的控制速度cmd_vel
*/
#include "motion_controller/motion_controller.hpp"

#include "base_interface/usr_text_output.hpp"

std::ofstream outfile_rectraj;
// std::ofstream outfile_motorsVel;
// std::ofstream outfile_Option;
std::ofstream test_file;

std::ofstream velocity_pid_compensate;
/**
 * @brief Motion_Controller节点构造函数
*/
Motion_Controller::Motion_Controller() : Node("Motion_Controller")
{
    RCLCPP_INFO(this->get_logger(),"节点Motion_Controller创建");
    //开启定时器，测试电机sin曲线用
    timer_ = this->create_wall_timer(5ms,std::bind(&Motion_Controller::test_timecallback, this));
    //订阅规划模块传递的轨迹
    recTraj = this->create_subscription<base_interface::msg::TrajectoryCustom>("traj_generate",10,std::bind(&Motion_Controller::TrajCallback,this,std::placeholders::_1));
    //订阅下位机传递的信息，比如位置坐标、偏航角等 （下位机：slave computer）
    recSlaveMsg = this->create_subscription<base_interface::msg::Ops>("/ops_data",10,std::bind(&Motion_Controller::RecSlaveMsg,this,std::placeholders::_1));
    //发布上位机minipc计算的控制指令 （底盘四个电机各自的速度）
    pub_Cmdvel = this->create_publisher<base_interface::msg::ControlCmd>("/cmd_vel",10);
    pub_robot_traj = this->create_publisher<visualization_msgs::msg::MarkerArray>("rviz_robot_traj",10);
    
    //把D给0 先调P 再慢慢给I
    float pos_kp = 2.0;
    float pos_ki = 0.5;
    float pos_kd = 0.8;

    float vel_kp = 0.5;
    float vel_ki = 0.0;
    float vel_kd = 0.0;

    pos_pid_ptr = std::make_shared<PidCompute>(pos_kp, pos_ki, pos_kd);
    vel_pid_ptr = std::make_shared<PidCompute>(vel_kp, vel_ki, vel_kd);

    std::make_shared<FileOstream>(outfile_rectraj, "data", "x_motion_control_txt", "outfile_rectraj");
    // std::make_shared<FileOstream>(outfile_motorsVel, "data", "x_motion_control_txt", "outfile_motorsVel");
    // std::make_shared<FileOstream>(outfile_Option, "data", "x_motion_control_txt", "outfile_Option");
    std::make_shared<FileOstream>(velocity_pid_compensate, "data", "x_motion_control_txt", "velocity_pid_compensate");
    std::make_shared<FileOstream>(test_file, "data", "test", "test_file");

    

}



float test_resultant_vel = 0;//合速度大小
float test_resultant_vel_angle = 0;//合速度方向
uint32_t ii;
double second_;
//0度方向  最大加速度：4.6m/s²
//45度方向  最大加速度：3.5m/s²
void Motion_Controller::test_timecallback()//每5ms执行一次
{
   ii++;
   second_ = ii*0.005;
// //擦轮子程序----------------------------------------------------------------------------------------
//     //合速度大小
//     test_resultant_vel = 0.1;
//     //合速度方向
//     test_resultant_vel_angle = 180.f/180.f*PI;
//     Chassis_Speed_Decomposition(test_resultant_vel, test_resultant_vel_angle, 0, 0);
}


/**
 * @brief 轨迹订阅函数
*/
void Motion_Controller::TrajCallback(const base_interface::msg::TrajectoryCustom::SharedPtr traj_custom)
{
    count++;
    RCLCPP_INFO(this->get_logger(),"第 %ld 次接收规划模块传递的速度*******",count);
    if(count > 300){
        count = 0;
    }

    PosVel_Matrix.resize(traj_custom->poses.size(), 4);
    PosVel_Matrix.setZero();

    float distance_length = 0.0;
    plan_length_index.clear();//清空数据
    plan_length_index.resize(traj_custom->poses.size()-1);

//   // 计算发送-接收延时
//     double traj_publish_time = double (traj_custom->header.stamp.sec) + double (traj_custom->header.stamp.nanosec)/1000000000.0f;
//     rclcpp::Time traj_receive_time;
//     traj_receive_time = this->now();
//     double delta_time = traj_receive_time.seconds()-traj_publish_time;
//     RCLCPP_INFO(this->get_logger(),"速度曲线发送和接收延时：%lf---------",delta_time);
//   double current_time = double(traj_custom->header.stamp.sec) + double(traj_custom->header.stamp.nanosec) / 1000000000.0f;
//   RCLCPP_INFO(this->get_logger(),"current_time：%lf---------",current_time);
//   // cout<<"current_time"<<"      "<<current_time<<endl;
//   // 计算发送-接收延时
//   double temp_time = (this->get_clock()->now().nanoseconds() - traj_custom->header.stamp.nanosec) / 1000000000.0;
//   RCLCPP_INFO(this->get_logger(),"速度曲线发送和接收延时：%lf---------",temp_time);
    
    //将接收的轨迹装入矩阵
    for(int i=0; i < traj_custom->poses.size(); i++){
        PosVel_Matrix(i, 0) = NumDecimal( traj_custom->poses[i].pose.position.x, 5);//x
        PosVel_Matrix(i, 1) = NumDecimal( traj_custom->poses[i].pose.position.y, 5);//y
        PosVel_Matrix(i, 2) = NumDecimal( traj_custom->velocities[i].linear.x  , 5);  //v_x
        PosVel_Matrix(i, 3) = NumDecimal( traj_custom->velocities[i].linear.y  , 5);  //v_y

        outfile_rectraj << PosVel_Matrix(i, 0) << "\t"<<PosVel_Matrix(i, 1) <<"\t"<< PosVel_Matrix(i, 2)<<"\t" << PosVel_Matrix(i, 3) << endl; 
    }
    outfile_rectraj<<endl;
    //从本次轨迹规划的起点到第i个点的路径长度
    for(int i = 0; i < traj_custom->poses.size()-1; i++){
        distance_length += sqrt(  pow(PosVel_Matrix(i + 1, 0) - PosVel_Matrix(i, 0), 2) 
                                + pow(PosVel_Matrix(i + 1, 1) - PosVel_Matrix(i, 1), 2));
        plan_length_index[i] = distance_length;
    }

//-------------------------------------------------------------------------------------------------------------------------------------------
    //开始跟踪轨迹的标志位,接收到规划出来的轨迹信息后 置1，跟踪完轨迹置0
    traj_Trak_flag = 1;
    // 每次轨迹跟踪任务开始的标志位,记录下新的轨迹跟踪任务开始时的时间值
    strat_TimeStamp_flag = 1;
    //pid积分清零   
    pos_pid_ptr->integral = 0.0;
    vel_pid_ptr->integral = 0.0;
    //delt_t时间间隔车移动的距离清零
    delt_s = 0.0;
    //从起点到当前时刻车移动的实际总距离清零
    motion_length = 0.0;
    //从起点到当前时刻车移动的期望总距离清零
    plan_length = 0.0;
}



/**
 * @brief 获得 轨迹跟踪任务开始时刻 到 当前时刻的时间差   并 完成轨迹跟踪 
 * 接收定位系统发送的数据，每5m定位系统发送一次
*/
//连接定位系统后可以进入
_Float64 time_ops;
void Motion_Controller::RecSlaveMsg(base_interface::msg::Ops::SharedPtr slave_msg)
{
    if(strat_TimeStamp_flag == 1){
        new_Trak_task_stamp = this->now();
        velocity_pid_compensate <<endl;

    }
    float time_past_real = this->now().seconds() - new_Trak_task_stamp.seconds();
    ComputeCtrl(slave_msg->x, slave_msg->y, slave_msg->x_vel, slave_msg->y_vel, PosVel_Matrix, time_past_real, slave_msg->theta, strat_TimeStamp_flag);
    strat_TimeStamp_flag = 0;

    //v合速度=1m/s时，最大加速度：4.6m/s²  最小加速度3.5m/s²
    // outfile_Option<<slave_msg->x<<"\t"<<slave_msg->y<<"\t"<<slave_msg->x_vel<<"\t"<<slave_msg->y_vel<<"\t"<<"time_ops:"<<"\t"<<time_ops<<"\t"<<"pos_combine:"<<"\t"<<sqrt(pow(slave_msg->x,2) + pow(slave_msg->y,2))<<"\t"<<"v_combine:"<<"\t"<<sqrt(pow(slave_msg->x_vel,2) + pow(slave_msg->y_vel,2))<<"\t"<<endl;
}


int state_s;
/**
 * @brief   控制量的计算---也应该是个回调函数，下位机传递数据到此回调函数
 * @param   pos_x        定位系统此刻x
 * @param   pos_y        定位系统此刻y
 * @param   PosVel_Matrix 存放每个0.005s的轨迹的位置速度数据的矩阵
 * @param   time         本次轨迹跟踪的实际时长
 * @param   ops_yaw      定位系统此刻的yaw
 * @param   clear_pre_pos_flag      即 strat_TimeStamp_flag
 */
void Motion_Controller::ComputeCtrl(const float& pos_x, const float& pos_y,
                                    const float& vel_x, const float& vel_y,
                                    const MatrixXd& PosVel_Matrix,
                                    const float& time_past_real, 
                                    const float& ops_yaw, 
                                    bool clear_pre_pos_flag)
{
    //rviz可视化
    // Rviz_RobotTraj(pos_x, pos_y, Target_Msg.TPos_x, Target_Msg.TPos_y);
    Rviz_RobotTraj(pos_x_last, pos_y_last, Target_Msg.TPos_x, Target_Msg.TPos_y);


    // cout<<"PosVel_Matrix的大小:"<<PosVel_Matrix.rows()<<endl;
    if(traj_Trak_flag == 1)
    {
    
        if(clear_pre_pos_flag == 1){
            // pos_x_last = pos_x;
            // pos_y_last = pos_y;
            //这样写的目的是消除轨迹发布和跟随接收之间时间差的影响，因为轨迹在发布的时候，车还是在移动的，因此当跟随模块接收的轨迹的时候，车的真实位置已经不在规划的起点了
            pos_x_last = PosVel_Matrix(0, 0);
            pos_y_last = PosVel_Matrix(0, 1);
        }
//----------------------------------------时间索引--------------------------
        float time_sum; //当前迭代时长 
        for(int i = 0; i < PosVel_Matrix.rows(); i++){
            time_sum = delt_t * i;
        //索引值时当前值所在行
            if(time_sum < time_past_real){
                if (i >= PosVel_Matrix.rows() - 1){
                    time_index = PosVel_Matrix.rows()-1;
                    break;
                }else
                    continue;
            }else{    
                time_index = i + 1;
                break;
            }
        }
//------------------------------------位置索引------------------------------------
        delt_s = sqrt(pow(pos_x - pos_x_last, 2) + pow(pos_y -pos_y_last, 2));
        motion_length += delt_s;
        for(int i = 0; i < PosVel_Matrix.rows() - 1; i++){
            plan_length = plan_length_index[i];
            if(motion_length >= plan_length)
                continue;
            else{
                pos_index = i + 1;
                break;
            }
        }

        tar_index = pos_index + 5;

        //防止溢出
        // 轨迹跟踪任务完成，不再进入轨迹跟踪函数
        if(time_index >= PosVel_Matrix.rows() - 1){
            time_index = PosVel_Matrix.rows() - 1;
        }
        if(pos_index >= PosVel_Matrix.rows() - 1){
            pos_index = PosVel_Matrix.rows() - 1;
        }

        if(tar_index >= PosVel_Matrix.rows() - 1){
            tar_index = PosVel_Matrix.rows() - 1;
        }


// cout<<"time_index: "<<time_index<<" pos_index: "<<pos_index<<" tar_index: "<<tar_index<<endl;

        pos_x_last = pos_x;
        pos_y_last = pos_y;
//----------------------------------------------------------跟随-----------------------------------------------------------
        // //时间索引指向的点
        Target_Msg.TPos_x = PosVel_Matrix(time_index, 0);
        Target_Msg.TPos_y = PosVel_Matrix(time_index, 1);

        //目标点
        Target_Msg.Tar_Px = PosVel_Matrix(tar_index, 0);//目标点的位置
        Target_Msg.Tar_Py = PosVel_Matrix(tar_index, 1);
        Target_Msg.Tar_Vx = PosVel_Matrix(tar_index, 2);//目标点的速度
        Target_Msg.Tar_Vy = PosVel_Matrix(tar_index, 3);

        //位置索引指向的点 匹配点 当前车的位置点 对应的 规划轨迹的位置点
        Target_Msg.PPos_x = PosVel_Matrix(pos_index, 0);
        Target_Msg.PPos_y = PosVel_Matrix(pos_index, 1); 

        Target_Msg.PPos_vx = PosVel_Matrix(pos_index, 2);
        Target_Msg.PPos_vy = PosVel_Matrix(pos_index, 3);

        Target_Msg.to_P_pid_vx = pos_pid_ptr->PidCtrl(pos_x, Target_Msg.PPos_x);
        Target_Msg.to_P_pid_vy = pos_pid_ptr->PidCtrl(pos_y, Target_Msg.PPos_y);

        Target_Msg.to_V_pid_vx = vel_pid_ptr->PidCtrl(vel_x,  (Target_Msg.Tar_Vx + Target_Msg.to_P_pid_vx) );
        Target_Msg.to_V_pid_vy = vel_pid_ptr->PidCtrl(vel_y,  (Target_Msg.Tar_Vy + Target_Msg.to_P_pid_vy) );

        //pid限幅
        float BOUND = 0.5;
        SetBound( Target_Msg.to_P_pid_vx, BOUND);
        SetBound( Target_Msg.to_P_pid_vy, BOUND);
        SetBound( Target_Msg.to_V_pid_vx, BOUND);
        SetBound( Target_Msg.to_V_pid_vy, BOUND);

        Target_Msg.out_vx = Target_Msg.Tar_Vx + Target_Msg.to_P_pid_vx + Target_Msg.to_V_pid_vx;
        Target_Msg.out_vy = Target_Msg.Tar_Vy + Target_Msg.to_P_pid_vy + Target_Msg.to_V_pid_vy;


        // 情况一：时间到了，位置没到，跟随慢了
        if(time_index > pos_index)
            state_s = -1;
        else if(time_index == pos_index)
            state_s = 0;
        // 情况二：位置到了，时间没到，跟随快了
        else
            state_s = 1;

        //合速度大小
        cmd_vel = sqrt( pow(Target_Msg.out_vx, 2) + pow(Target_Msg.out_vy, 2));
        //合速度方向  
        cmd_vel_angle = atan2( NumDecimal(Target_Msg.out_vy, 3) , NumDecimal(Target_Msg.out_vx , 3));
            
        if(time_index >= PosVel_Matrix.rows() - 1){//时间结束 跟踪完成
        
            cmd_vel = 0;
            traj_Trak_flag = 0;
            cout<<"***********************"<<endl;
            cout<<"time_index:"<< time_index<<" ptx: "<< PosVel_Matrix(time_index, 0)<<" pty: " <<PosVel_Matrix(time_index, 1)<<endl;
            cout<<"pos_index: "<<pos_index<<" ppx: "<< PosVel_Matrix(pos_index, 0)<<" ppy: " <<PosVel_Matrix(pos_index, 1) <<endl;                        
            cout<<"plan_length: "<<plan_length<<" motion_length: "<<motion_length<<endl;
            cout<<endl;
        }
    }

    else{ //traj_Trak_flag == 0  
        cmd_vel = 0;
    }


    const float corr_kp = 0.5; // 3
    const float corr_ki = 0;
    const float corr_kd = 0.3; // 2.5

    // 机器人偏航角也就是方向，闭环 PID，需要根据偏航角方向的正反来确定下面公式的计算
    error_spin = 0 - ops_yaw;
        
    Target_Msg.OUT_Corr = corr_kp * (error_spin) + corr_kd * (error_spin - error_spin_last);

    error_spin_last = error_spin;


if(traj_Trak_flag == 1){
    velocity_pid_compensate <<"  cmd_vel: \t"          <<cmd_vel                          
                            <<"\tangle: \t"             <<cmd_vel_angle/PI*180.f        

                            <<"\ttime_index \t"         <<time_index                      
                            <<"\tpos_index \t"          <<pos_index                           
                            <<"\ttar_index \t"          <<tar_index  

                            <<"\t"
                            <<"\tindex_Vx \t"           <<Target_Msg.Tar_Vx                 
                            <<"\ttP pidVx \t"           <<Target_Msg.to_P_pid_vx             
                            <<"\ttV pidVx: \t"          <<Target_Msg.to_V_pid_vx          
                            <<"\tOut_vx \t"             <<Target_Msg.out_vx 
                            <<"\tvel_x \t"              <<vel_x
                            <<"\t▲vx:\t"                <<(Target_Msg.Tar_Vx + Target_Msg.to_P_pid_vx) - vel_x

                            <<"\t"
                            <<"\tindex_Vy \t"           <<Target_Msg.Tar_Vy                
                            <<"\ttP pidVy: \t"          <<Target_Msg.to_P_pid_vy            
                            <<"\ttV pidVy: \t"          <<Target_Msg.to_V_pid_vy        
                            <<"\tOut_vy \t"             <<Target_Msg.out_vy  
                            <<"\tvel_y \t"              <<vel_y
                            <<"\t▲vy:\t"                <<(Target_Msg.Tar_Vy + Target_Msg.to_P_pid_vy) - vel_y  

                            // <<"\t plan_s: \t"            <<plan_length                      
                            // <<"\t move_s: \t"            <<motion_length                        
                            // <<"\t 慢/等/快(-1/0/1): \t"   <<state_s                                
                            // <<"\t 滞后的时长: \t"          <<(time_index-pos_index)*delt_t     
                            
                            <<"\t"
                            <<"\tTPos_x :\t"             <<Target_Msg.TPos_x
                            <<"\tTar_Px \t"              <<Target_Msg.Tar_Px 
                            <<"\tPPos_x \t"              <<Target_Msg.PPos_x                 
                            <<"\tpos_x \t"               <<pos_x  
                            <<"\t▲Px:\t"                 <<Target_Msg.PPos_x - pos_x
                            <<"\t▲Tx(cm)看终点:\t"            <<(Target_Msg.TPos_x - pos_x)*100

                            <<"\t"
                            <<"\tTPos_y :\t"             <<Target_Msg.TPos_y                  
                            <<"\tTar_Py \t"              <<Target_Msg.Tar_Py             
                            <<"\tPPos_y \t"              <<Target_Msg.PPos_y  
                            <<"\tpos_y \t"               <<pos_y 
                            <<"\t▲Py:\t"                 <<Target_Msg.PPos_y - pos_y
                            <<"\t▲Ty(cm)看终点:\t"            <<(Target_Msg.TPos_y - pos_y)*100  

                            << endl;                              
}

    //将合速度进行底盘分解
    Chassis_Speed_Decomposition(cmd_vel, cmd_vel_angle, ops_yaw, Target_Msg.OUT_Corr);//0.35   
}

/**
 * @brief 底盘分解
 * @param cmd_vel_ 合速度大小
 * @param cmd_vel_angle_  合速度方向
 * @param ops_yaw_ 定位系统偏航角
 * @param omega Target_Msg.OUT_Corr
*/

void Motion_Controller::Chassis_Speed_Decomposition(float cmd_vel_, float cmd_vel_angle_,float ops_yaw_, float omega)
{
    // cout<< "in Chassis_Speed_Decompostion"<<endl;
    base_interface::msg::ControlCmd controlcmd;
    float theta = cmd_vel_angle_ - ops_yaw_;
	// 让底盘逆时针转动时，四个电机转动方向为正方向
    // 单位 m/s
    controlcmd.target_one_motor_vel = -cmd_vel_ * cos(PI/4 + theta) + omega * AXIS_LENGTH;
    controlcmd.target_two_motor_vel = -cmd_vel_ * cos(PI/4 - theta) + omega * AXIS_LENGTH;
    controlcmd.target_thr_motor_vel = +cmd_vel_ * cos(PI/4 + theta) + omega * AXIS_LENGTH;
    controlcmd.target_fou_motor_vel = +cmd_vel_ * cos(PI/4 - theta) + omega * AXIS_LENGTH;

	// 单位 从m/s转换到r/s    频率f = v/(2*PI*R)
    controlcmd.target_one_motor_vel = NumDecimal(controlcmd.target_one_motor_vel / (2*PI*WHEEL_RADIUS) , 3);
    controlcmd.target_two_motor_vel = NumDecimal(controlcmd.target_two_motor_vel / (2*PI*WHEEL_RADIUS) , 3);
    controlcmd.target_thr_motor_vel = NumDecimal(controlcmd.target_thr_motor_vel / (2*PI*WHEEL_RADIUS) , 3);
    controlcmd.target_fou_motor_vel = NumDecimal(controlcmd.target_fou_motor_vel / (2*PI*WHEEL_RADIUS) , 3);



    pub_Cmdvel->publish(controlcmd);
}



// ----------------------------------------------------------------Rviz2轨迹可视化----------------------------------------------------------------------------
/**
 * @brief Rviz2轨迹可视化示例
 * @param real_x,real_y 实际x,y
 * @param tar_x,tar_y 目标x,t
*/
void Motion_Controller::Rviz_RobotTraj(float real_x, float real_y, float tar_x, float tar_y)
{

    visualization_msgs::msg::MarkerArray traj_marker_array;
    geometry_msgs::msg::Point p_tar;      //规划的目标点位置
    geometry_msgs::msg::Point p_real;      //车实际的位置

    traj_tar.header.frame_id = "map";
    traj_tar.ns = "traj_tar";
    traj_tar.id = 0;
    traj_tar.type = visualization_msgs::msg::Marker::POINTS;
    traj_tar.action = visualization_msgs::msg::Marker::ADD;
    traj_tar.scale.x = 0.005;
    traj_tar.scale.y = 0.005;
    traj_tar.scale.z = 0.001;
    traj_tar.color.r = 1.0;
    traj_tar.color.g = 0.0;
    traj_tar.color.b = 0.0;
    traj_tar.color.a = 0.5;
    p_tar.x = tar_x;
    p_tar.y = tar_y;
    traj_tar.points.emplace_back(p_tar);

    traj_real.header.frame_id = "map";
    traj_real.ns = "traj_real";
    traj_real.id = 1;
    traj_real.type = visualization_msgs::msg::Marker::POINTS;
    traj_real.action = visualization_msgs::msg::Marker::ADD;
    traj_real.scale.x = 0.005;
    traj_real.scale.y = 0.005;
    traj_real.scale.z = 0.005;
    traj_real.color.r = 0.0;
    traj_real.color.g = 1.0;
    traj_real.color.b = 0.0;
    traj_real.color.a = 0.5;
    p_real.x = real_x;
    p_real.y = real_y;
    traj_real.points.emplace_back(p_real);

    traj_marker_array.markers.emplace_back(traj_tar);
    traj_marker_array.markers.emplace_back(traj_real);

    //发布规划和实际的轨迹数组
    pub_robot_traj->publish(traj_marker_array);

}



/**
 * @brief 保留小数点后的数字的个数
 * @param value 数字
 * @param n_ 小数点后要保留数字的个数
*/
float NumDecimal(float value,int n_)
{
    return (float)((int)( value * pow(10,n_)))/pow(10,n_);
}

/**
 * @brief 限幅
*/
void SetBound(float& value, float max)
{
    if( value > max)
        value = max;
    if( value < - max)
        value = - max;
}
